#include"ArmorDetectorT.h"


namespace hnurm
{
    ArmorDetector::ArmorDetector()
    {
        _kernel=cv::getStructuringElement(cv::MORPH_CROSS,cv::Size(4,4));
    }

    int ArmorDetector::Detect(const ImgInfo& raw_frame,TargetInfo& target)
    {
        _carrier=raw_frame.img;
        _raw_img=_carrier;
        Preprocess();
        CreateBars();
        TargetInfo possible_armor;
        CreateTarget(_light_bars[0],_light_bars[1],possible_armor);
        // int nums=PairBars();
        // ChooseTarget();
        return 1;
    }

    void ArmorDetector::Preprocess()
    {
        // get grey
        cv::cvtColor(_carrier,_carrier,cv::COLOR_BGR2GRAY);
        // binarization processing
        cv::threshold(_carrier,_carrier,240,255,0);
    }


    void ArmorDetector::CreateBars()
    {
        cv::Mat _canvas = cv::Mat::zeros(_carrier.size(),CV_8UC1);
        cv::findContours(_carrier,_contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
        for(auto i =0;i<_contours.size();i++)
        {
            for(auto j=0;j<_contours[i].size();j++)
            {
                circle(_canvas,_contours[i][j],0.1,cv::Scalar(255));
            }
        }
        for (int i = 0; i < _contours.size(); i++)
        {
            double area = cv::contourArea(_contours[i]);
            if(area < 10|| area>20000)
                continue;
            if(_contours[i].size()<5)
                continue;
            _tmp_rect= cv::fitEllipse(_contours[i]);
            _light_bars.push_back(_tmp_rect);
        }
        // if(_light_bars.size()==2)
        // {
        //     DrawRect(_raw_img,_light_bars[0],_light_bars[1]);
        // }
    }
    


    void ArmorDetector::CreateTarget(const cv::RotatedRect& rect1,const cv::RotatedRect& rect2,TargetInfo& target)
    {
        // treat the center of width to be a vertex
        // tell the left-right using center.x
        // ...
        cv::Point2f v1[4];
        cv::Point2f v2[4];
        if(rect1.angle>100&&rect2.angle>100)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[1].x)
            {
                rect1.points(v1);
                rect2.points(v2);
            }
            else
            {
                rect1.points(v2);
                rect2.points(v1);
            } 
            target.vertexes.push_back(v2[0]);
            target.vertexes.push_back(v1[3]);
            target.vertexes.push_back(v1[2]);
            target.vertexes.push_back(v2[1]); 
            std::cout<<v2[0]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[1]<<std::endl;
        }
        else if(rect1.angle<80&&rect2.angle<80)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[3].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
            }
            else
            {
                rect1.points(v2);
                rect2.points(v1);
            }
            target.vertexes.push_back(v2[2]);
            target.vertexes.push_back(v1[1]);
            target.vertexes.push_back(v1[0]);
            target.vertexes.push_back(v2[3]); 
            std::cout<<v2[2]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[3]<<std::endl;
        }
        else if(rect1.angle>100&&rect2.angle<80)
        {

            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
                target.vertexes.push_back(v2[2]);
                target.vertexes.push_back(v1[3]);
                target.vertexes.push_back(v1[2]);
                target.vertexes.push_back(v2[3]); 
                std::cout<<v2[2]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[3]<<std::endl;
            }
            else if(v3[1].x<v4[3].x)
            {
                rect1.points(v2);
                rect2.points(v1);
                target.vertexes.push_back(v2[0]);
                target.vertexes.push_back(v1[1]);
                target.vertexes.push_back(v1[0]);
                target.vertexes.push_back(v2[1]); 
                std::cout<<v2[0]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[1]<<std::endl;
            }
        }
        else if(rect1.angle<80&&rect2.angle>100)
        {
            cv::Point2f v3[4];
            rect1.points(v3);
            cv::Point2f v4[4];
            rect2.points(v4);
            if(v3[1].x>v4[3].x)
            {
                rect1.points(v1);
                rect2.points(v2);
                target.vertexes.push_back(v2[0]);
                target.vertexes.push_back(v1[1]);
                target.vertexes.push_back(v1[0]);
                target.vertexes.push_back(v2[1]); 
                std::cout<<v2[0]<<" "<<v1[1]<<" "<<v1[0]<<" "<<v2[1]<<std::endl;
            }
            else if(v3[1].x<v4[3].x)
            {
                rect1.points(v2);
                rect2.points(v1);
                target.vertexes.push_back(v2[2]);
                target.vertexes.push_back(v1[3]);
                target.vertexes.push_back(v1[2]);
                target.vertexes.push_back(v2[3]); 
                std::cout<<v2[2]<<" "<<v1[3]<<" "<<v1[2]<<" "<<v2[3]<<std::endl;
            }
        }
    }

    void ArmorDetector::Reset()
    {
        _contours.clear();
        _light_bars.clear();
        _armors.clear();   
    }

} // namespace hnurm

